
/*
 LED > 3V3
 SCK > 13
 SDA > 11
 A0 (DC) > 9
 RESET > 8
 CS > 10
 GND > GND
 VCC > 5V
 */


/***************************************************
	This is a library for the Adafruit 1.8" SPI display.

This library works with the Adafruit 1.8" TFT Breakout w/SD card
	----> http://www.adafruit.com/products/358
The 1.8" TFT shield
	----> https://www.adafruit.com/product/802
The 1.44" TFT breakout
	----> https://www.adafruit.com/product/2088
as well as Adafruit raw 1.8" TFT display
	----> http://www.adafruit.com/products/618

	Check out the links above for our tutorials and wiring diagrams
	These displays use SPI to communicate, 4 or 5 pins are required to
	interface (RST is optional)
	Adafruit invests time and resources providing this open source code,
	please support Adafruit and open-source hardware by purchasing
	products from Adafruit!

	Written by Limor Fried/Ladyada for Adafruit Industries.
	MIT license, all text above must be included in any redistribution
 ****************************************************/

#include <Adafruit_GFX.h>		// Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library for ST7735
#include <Adafruit_ST7789.h> // Hardware-specific library for ST7789
#include <SPI.h>

#include "Arduino_BMI270_BMM150.h"

#include <TensorFlowLite.h>
#include <tensorflow/lite/micro/all_ops_resolver.h>
#include <tensorflow/lite/micro/micro_error_reporter.h>
#include <tensorflow/lite/micro/micro_interpreter.h>
#include <tensorflow/lite/schema/schema_generated.h>
#include <tensorflow/lite/version.h>

#include "model3_60_60.h"

const float accelerationThreshold = 2.5; // threshold of significant in G's
const int numSamples = 119;

int samplesRead = numSamples;
// For the breakout, you can use any 2 or 3 pins
// These pins will also work for the 1.8" TFT shield
#define TFT_CS		 10
#define TFT_RST		8	// you can also connect this to the Arduino reset
											// in which case, set this #define pin to -1!
#define TFT_DC		9

// Option 1 (recommended): must use the hardware SPI pins
// (for UNO thats sclk = 13 and sid = 11) and pin 10 must be
// an output. This is much faster - also required if you want
// to use the microSD card (see the image drawing example)

// For 1.44" and 1.8" TFT with ST7735 use
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS,	TFT_DC, TFT_RST);

// For 1.54" TFT with ST7789
//Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS,	TFT_DC, TFT_RST);

// Option 2: use any pins but a little slower!
//#define TFT_SCLK 13	 // set these to be whatever pins you like!
//#define TFT_MOSI 11	 // set these to be whatever pins you like!
//Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);


float p = 3.1415926;

// global variables used for TensorFlow Lite (Micro)
tflite::MicroErrorReporter tflErrorReporter;

// pull in all the TFLM ops, you can remove this line and
// only pull in the TFLM ops you need, if would like to reduce
// the compiled size of the sketch.
tflite::AllOpsResolver tflOpsResolver;

const tflite::Model* tflModel = nullptr;
tflite::MicroInterpreter* tflInterpreter = nullptr;
TfLiteTensor* tflInputTensor = nullptr;
TfLiteTensor* tflOutputTensor = nullptr;

// Create a static memory buffer for TFLM, the size may need to
// be adjusted based on the model3 you are using
constexpr int tensorArenaSize = 8 * 1024;
byte tensorArena[tensorArenaSize] __attribute__((aligned(16)));

// array to map gesture index to a name
const char* GESTURES[] = {
	"punch",
	"flex",
	"roll"
};

#define NUM_GESTURES (sizeof(GESTURES) / sizeof(GESTURES[0]))

void setup(void) {
	Serial.begin(9600);
	Serial.print("Hello! ST77xx TFT Test");

	// Use this initializer if you're using a 1.8" TFT
	tft.initR(INITR_BLACKTAB);	 // initialize a ST7735S chip, black tab

	// Use this initializer (uncomment) if you're using a 1.44" TFT
	//tft.initR(INITR_144GREENTAB);	 // initialize a ST7735S chip, black tab

	// Use this initializer (uncomment) if you're using a 0.96" 180x60 TFT
	//tft.initR(INITR_MINI160x80);	 // initialize a ST7735S chip, mini display

	// Use this initializer (uncomment) if you're using a 1.54" 240x240 TFT
	//tft.init(240, 240);	 // initialize a ST7789 chip, 240x240 pixels

	Serial.println("Initialized");

	uint16_t time = millis();
	tft.fillScreen(ST77XX_BLACK);
	time = millis() - time;

	Serial.println(time, DEC);
	delay(500);

	// large block of text
	tft.fillScreen(ST77XX_BLACK);
	testdrawtext("Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabitur adipiscing ante sed nibh tincidunt feugiat. Maecenas enim massa, fringilla sed malesuada et, malesuada sit amet turpis. Sed porttitor neque ut ante pretium vitae malesuada nunc bibendum. Nullam aliquet ultrices massa eu hendrerit. Ut sed nisi lorem. In vestibulum purus a tortor imperdiet posuere. ", ST77XX_WHITE);
	delay(1000);

	// tft print function!
	// tftPrintTest();
	// delay(4000);

	// // a single pixel
	// tft.drawPixel(tft.width()/2, tft.height()/2, ST77XX_GREEN);
	// delay(500);

	// // line draw test
	// testlines(ST77XX_YELLOW);
	// delay(500);

	// // optimized lines
	// testfastlines(ST77XX_RED, ST77XX_BLUE);
	// delay(500);

	// testdrawrects(ST77XX_GREEN);
	// delay(500);

	// testfillrects(ST77XX_YELLOW, ST77XX_MAGENTA);
	// delay(500);

	// tft.fillScreen(ST77XX_BLACK);
	// testfillcircles(10, ST77XX_BLUE);
	// testdrawcircles(10, ST77XX_WHITE);
	// delay(500);

	// testroundrects();
	// delay(500);

	// testtriangles();
	// delay(500);

	// mediabuttons();
	// delay(500);


	if (!IMU.begin()) {
		Serial.println("Failed to initialize IMU!");
		while (1);
	}

	// print out the samples rates of the IMUs
	Serial.print("Accelerometer sample rate = ");
	Serial.print(IMU.accelerationSampleRate());
	Serial.println(" Hz");
	Serial.print("Gyroscope sample rate = ");
	Serial.print(IMU.gyroscopeSampleRate());
	Serial.println(" Hz");

	Serial.println();

	// get the TFL representation of the model3 byte array
	tflModel = tflite::GetModel(model);
	if (tflModel->version() != TFLITE_SCHEMA_VERSION) {
		Serial.println("Model schema mismatch!");
		while (1);
	}

	// Create an interpreter to run the model3
	tflInterpreter = new tflite::MicroInterpreter(tflModel, tflOpsResolver, tensorArena, tensorArenaSize, &tflErrorReporter);

	// Allocate memory for the model3's input and output tensors
	tflInterpreter->AllocateTensors();

	// Get pointers for the model3's input and output tensors
	tflInputTensor = tflInterpreter->input(0);
	tflOutputTensor = tflInterpreter->output(0);

	Serial.println("done");
}

void loop() {
	float aX, aY, aZ, gX, gY, gZ;
	int a = 0;
	// wait for significant motion
	while (samplesRead == numSamples) {
		if (IMU.accelerationAvailable()) {
			// read the acceleration data
			IMU.readAcceleration(aX, aY, aZ);

			// sum up the absolutes
			float aSum = fabs(aX) + fabs(aY) + fabs(aZ);

			// check if it's above the threshold
			if (aSum >= accelerationThreshold) {
				// reset the sample read count
				samplesRead = 0;
				break;
			}
			if (a <= 0) {
				if (a == 0) {
					tft.fillScreen(ST77XX_BLACK);
					testdrawtext("flat", ST77XX_WHITE);
					a --;
				}
			}
			else{
				a --;
			}
		}
	}

	// check if the all the required samples have been read since
	// the last time the significant motion was detected
	while (samplesRead < numSamples) {
		// check if new acceleration AND gyroscope data is available
		if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable()) {
			// read the acceleration and gyroscope data
			IMU.readAcceleration(aX, aY, aZ);
			IMU.readGyroscope(gX, gY, gZ);

			// normalize the IMU data between 0 to 1 and store in the model3's
			// input tensor
			tflInputTensor->data.f[samplesRead * 6 + 0] = (aX + 4.0) / 8.0;
			tflInputTensor->data.f[samplesRead * 6 + 1] = (aY + 4.0) / 8.0;
			tflInputTensor->data.f[samplesRead * 6 + 2] = (aZ + 4.0) / 8.0;
			tflInputTensor->data.f[samplesRead * 6 + 3] = (gX + 2000.0) / 4000.0;
			tflInputTensor->data.f[samplesRead * 6 + 4] = (gY + 2000.0) / 4000.0;
			tflInputTensor->data.f[samplesRead * 6 + 5] = (gZ + 2000.0) / 4000.0;

			samplesRead++;

			if (samplesRead == numSamples) {
				// Run inferencing
				TfLiteStatus invokeStatus = tflInterpreter->Invoke();
				if (invokeStatus != kTfLiteOk) {
					Serial.println("Invoke failed!");
					while (1);
					return;
				}

				// Loop through the output tensor values from the model3
				float maxValue = 0.0;
				int maxI = 0;
				for (int i = 0; i < NUM_GESTURES; i++) {
					Serial.print(GESTURES[i]);
					Serial.print(": ");
					Serial.println(tflOutputTensor->data.f[i], 6);
					if (tflOutputTensor->data.f[i] > maxValue) {
						maxValue = tflOutputTensor->data.f[i];
						maxI = i;
					}
				}
				tft.fillScreen(ST77XX_BLACK);
				testdrawtext(const_cast<char*>(GESTURES[maxI]), ST77XX_WHITE);
				Serial.println();
				a = 1000;
			}
		}
	}
}

void testlines(uint16_t color) {
	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=0; x < tft.width(); x+=6) {
		tft.drawLine(0, 0, x, tft.height()-1, color);
	}
	for (int16_t y=0; y < tft.height(); y+=6) {
		tft.drawLine(0, 0, tft.width()-1, y, color);
	}

	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=0; x < tft.width(); x+=6) {
		tft.drawLine(tft.width()-1, 0, x, tft.height()-1, color);
	}
	for (int16_t y=0; y < tft.height(); y+=6) {
		tft.drawLine(tft.width()-1, 0, 0, y, color);
	}

	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=0; x < tft.width(); x+=6) {
		tft.drawLine(0, tft.height()-1, x, 0, color);
	}
	for (int16_t y=0; y < tft.height(); y+=6) {
		tft.drawLine(0, tft.height()-1, tft.width()-1, y, color);
	}

	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=0; x < tft.width(); x+=6) {
		tft.drawLine(tft.width()-1, tft.height()-1, x, 0, color);
	}
	for (int16_t y=0; y < tft.height(); y+=6) {
		tft.drawLine(tft.width()-1, tft.height()-1, 0, y, color);
	}
}

void testdrawtext(char *text, uint16_t color) {
	tft.setCursor(0, 0);
	tft.setTextColor(color);
	tft.setTextWrap(true);
	tft.print(text);
}

void testfastlines(uint16_t color1, uint16_t color2) {
	tft.fillScreen(ST77XX_BLACK);
	for (int16_t y=0; y < tft.height(); y+=5) {
		tft.drawFastHLine(0, y, tft.width(), color1);
	}
	for (int16_t x=0; x < tft.width(); x+=5) {
		tft.drawFastVLine(x, 0, tft.height(), color2);
	}
}

void testdrawrects(uint16_t color) {
	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=0; x < tft.width(); x+=6) {
		tft.drawRect(tft.width()/2 -x/2, tft.height()/2 -x/2 , x, x, color);
	}
}

void testfillrects(uint16_t color1, uint16_t color2) {
	tft.fillScreen(ST77XX_BLACK);
	for (int16_t x=tft.width()-1; x > 6; x-=6) {
		tft.fillRect(tft.width()/2 -x/2, tft.height()/2 -x/2 , x, x, color1);
		tft.drawRect(tft.width()/2 -x/2, tft.height()/2 -x/2 , x, x, color2);
	}
}

void testfillcircles(uint8_t radius, uint16_t color) {
	for (int16_t x=radius; x < tft.width(); x+=radius*2) {
		for (int16_t y=radius; y < tft.height(); y+=radius*2) {
			tft.fillCircle(x, y, radius, color);
		}
	}
}

void testdrawcircles(uint8_t radius, uint16_t color) {
	for (int16_t x=0; x < tft.width()+radius; x+=radius*2) {
		for (int16_t y=0; y < tft.height()+radius; y+=radius*2) {
			tft.drawCircle(x, y, radius, color);
		}
	}
}

void testtriangles() {
	tft.fillScreen(ST77XX_BLACK);
	int color = 0xF800;
	int t;
	int w = tft.width()/2;
	int x = tft.height()-1;
	int y = 0;
	int z = tft.width();
	for(t = 0 ; t <= 15; t++) {
		tft.drawTriangle(w, y, y, x, z, x, color);
		x-=4;
		y+=4;
		z-=4;
		color+=100;
	}
}

void testroundrects() {
	tft.fillScreen(ST77XX_BLACK);
	int color = 100;
	int i;
	int t;
	for(t = 0 ; t <= 4; t+=1) {
		int x = 0;
		int y = 0;
		int w = tft.width()-2;
		int h = tft.height()-2;
		for(i = 0 ; i <= 16; i+=1) {
			tft.drawRoundRect(x, y, w, h, 5, color);
			x+=2;
			y+=3;
			w-=4;
			h-=6;
			color+=1100;
		}
		color+=100;
	}
}

void tftPrintTest() {
	tft.setTextWrap(false);
	tft.fillScreen(ST77XX_BLACK);
	tft.setCursor(0, 30);
	tft.setTextColor(ST77XX_RED);
	tft.setTextSize(1);
	tft.println("Hello World!");
	tft.setTextColor(ST77XX_YELLOW);
	tft.setTextSize(2);
	tft.println("Hello World!");
	tft.setTextColor(ST77XX_GREEN);
	tft.setTextSize(3);
	tft.println("Hello World!");
	tft.setTextColor(ST77XX_BLUE);
	tft.setTextSize(4);
	tft.print(1234.567);
	delay(1500);
	tft.setCursor(0, 0);
	tft.fillScreen(ST77XX_BLACK);
	tft.setTextColor(ST77XX_WHITE);
	tft.setTextSize(0);
	tft.println("Hello World!");
	tft.setTextSize(1);
	tft.setTextColor(ST77XX_GREEN);
	tft.print(p, 6);
	tft.println(" Want pi?");
	tft.println(" ");
	tft.print(8675309, HEX); // print 8,675,309 out in HEX!
	tft.println(" Print HEX!");
	tft.println(" ");
	tft.setTextColor(ST77XX_WHITE);
	tft.println("Sketch has been");
	tft.println("running for: ");
	tft.setTextColor(ST77XX_MAGENTA);
	tft.print(millis() / 1000);
	tft.setTextColor(ST77XX_WHITE);
	tft.print(" seconds.");
}

void mediabuttons() {
	// play
	tft.fillScreen(ST77XX_BLACK);
	tft.fillRoundRect(25, 10, 78, 60, 8, ST77XX_WHITE);
	tft.fillTriangle(42, 20, 42, 60, 90, 40, ST77XX_RED);
	delay(500);
	// pause
	tft.fillRoundRect(25, 90, 78, 60, 8, ST77XX_WHITE);
	tft.fillRoundRect(39, 98, 20, 45, 5, ST77XX_GREEN);
	tft.fillRoundRect(69, 98, 20, 45, 5, ST77XX_GREEN);
	delay(500);
	// play color
	tft.fillTriangle(42, 20, 42, 60, 90, 40, ST77XX_BLUE);
	delay(50);
	// pause color
	tft.fillRoundRect(39, 98, 20, 45, 5, ST77XX_RED);
	tft.fillRoundRect(69, 98, 20, 45, 5, ST77XX_RED);
	// play color
	tft.fillTriangle(42, 20, 42, 60, 90, 40, ST77XX_GREEN);
}